#include <iostream>
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>


#include "constants.h"
#include "other_functions.h"

#include "view.h"
#include "model.h"

#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"


pose3D & pose3D::operator = (pose3D & pose)
{
    cvCopy(pose.translation, translation);
    cvCopy(pose.vect, vect);
    return *this;
}

void pose3D::resetR()
{
    cvZero(vect);
}
void pose3D::resetT()
{
    cvZero(translation);
}
void pose3D::rotate(float a, float b, float g)
{
    vect->data.fl[0]+=a;
    vect->data.fl[1]+=b;
    vect->data.fl[2]+=g;
}
void pose3D::translate(float x, float y, float z)
{
    translation->data.fl[0]+=x;
    translation->data.fl[1]+=y;
    translation->data.fl[2]+=z;
}
void pose3D::setrotate(float a, float b, float g)
{
    vect->data.fl[0]=a;
    vect->data.fl[1]=b;
    vect->data.fl[2]=g;
}
void pose3D::settranslate(float x, float y, float z)
{
    translation->data.fl[0]=x;
    translation->data.fl[1]=y;
    translation->data.fl[2]=z;
}
void pose3D::setTx(float x)
{
    translation->data.fl[0]=x;
}
void pose3D::setTy(float y)
{
    translation->data.fl[1]=y;
}
void pose3D::setTz(float z)
{
    translation->data.fl[2]=z;
}
    
void pose3D::fprintf_T(FILE *fout)
{
    ::fprintf(fout, "%f %f %f ",
	      cvmGet(T,0,0), cvmGet(T,1,0), cvmGet(T,2,0));

}

void pose3D::fprintf_R(FILE *fout)
{
    ::fprintf(fout, "%f %f %f ",
	      cvmGet(R,0,0), cvmGet(R,1,0),cvmGet(R,2,0));
}

void pose3D::fprintf(FILE *f)
{
    fprintf_R(f);
    fprintf_T(f);
}

void pose3D::ChangeView(pose3D &view2, CALIBRATION_PARAMS_t * calibration)
{
    cvRodrigues2(view2.R, view2.rotation);
    cvMatMul(calibration->R, view2.rotation, rotation);
    cvRodrigues2(rotation, R);
    cvMatMulAdd(calibration->R, view2.T, calibration->T, T);
}

float pose3D::distanceT2(pose3D &pose2)
{
    return ::distanceT2(*this, pose2);
}

float pose3D::distanceR(pose3D &pose2)
{
    return ::distanceR(*this, pose2);
}

float distanceT2(pose3D &pose1, pose3D &pose2)
{
    return distance3x1_2(pose1.T, pose2.T);
}

float distanceR(pose3D &pose1, pose3D &pose2)
{
    float dist2;
    float dist=0;
    for (int i=0; i<3; i++)
    {
	dist2 = cvmGet(pose1.R, i, 0) - cvmGet(pose2.R,i,0);
	if (dist2 <0)
	    dist2 = -dist2;
	dist += dist2;
    }
    return dist;
}

float distance3x1_2(CvMat * pose1, CvMat  * pose2)
{
    float dist2;
    float dist=0;
    for (int i=0; i<3; i++)
    {
	dist2 = cvmGet(pose1, i, 0) - cvmGet(pose2,i,0);
	dist += dist2*dist2;
    }
    return dist;
}

float pose3D::Rx ()
{
    return cvmGet(R, 0,0);
}
float pose3D::Ry ()
{
    return cvmGet(R, 1,0);
}
float pose3D::Rz ()
{
    return cvmGet(R, 2,0);
}

float pose3D::Tx ()
{
    return cvmGet(T, 0,0);
}
float pose3D::Ty ()
{
    return cvmGet(T, 1,0);
}
float pose3D::Tz ()
{
    return cvmGet(T, 2,0);
}

/*********************************************************
class CView
********************************************************/
void CView::alloc(int points)
{
    if (!mask_show)
	cvReleaseMat(&mask_show);
    mask_show = cvCreateMat(points, 1, CV_8UC1);	  	
}

CView & CView::operator = (CView & view)
{
    pose3D::operator=(view);
    if (mask_show)
	cvCopy(view.mask_show, mask_show);
    else 
	mask_show = cvCloneMat(view.mask_show);
    calibration = view.calibration;
    return *this;
}

void CView::setmask_show(CvMat * maskin)
{
    if (!maskin)
	cvSet(mask_show, cvScalar(1));
    else
	cvCopy(maskin, mask_show);
}
int CView::deletepoint(int index)
{
    return DeleteRow(&mask_show, index );
}
    
void CView::getNotHiden(CvMat * angY)
{
    //anulamos los puntos que se pasen del angulo correspondiente
    int i;
    float ang = cvmGet(vect, 1, 0);

    for (i=0; i<angY->rows; i++)
    {
	if (cvmGet(angY, i, 0) > ang && cvmGet(angY, i, 1) < ang)
	    cvSet1D(mask_show, i, cvScalar(255));
	else
	    cvSet1D(mask_show, i, cvScalar(0));
    }
}


void CView::addpoint()
{
    //change size of matrices
    AddRow2new(&mask_show);
}    



void CView::ChangeView(pose3D &view2)
{
    cvRodrigues2(view2.R, view2.rotation);
    cvMatMul(calibration->R, view2.rotation, rotation);
    cvRodrigues2(rotation, R);
    cvMatMulAdd(calibration->R, view2.T, calibration->T, T);
}
